/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    QT_Node.cpp
 * @author
 * @brief   Code for .
 * @date   2022-02-28
 * @version 1.0
 * @par Change Log:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author     <th>Description
 * </table>
 *
 ******************************************************************************
 * @attention
 *
 * if you had modified this file, please make sure your code does not have many
 * bugs, update the version Number, write dowm your name and the date, the most
 * important is make sure the users will have clear and definite understanding
 * through your new brief.
 *
 * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
 * All rights reserved.</center></h2>
 ******************************************************************************
 */
#include "ros_network/QT_Node.h"
#include "common/Controllers/LegController.h"
#include "common/Utilities/rc_interface.h"

void QT_Node::Node_Init(void) {
    FeetCmd_Suber = n->subscribe("/quadruped/qt_monitor/feet_cmd", 100, &QT_Node::FeetCmd_Callback, this);
    JointsCmd_Suber = n->subscribe("/quadruped/qt_monitor/joints_cmd", 100, &QT_Node::JointsCmd_Callback, this);
    Joystick_Suber = n->subscribe("/quadruped/qt_monitor/joystick_cmd", 100, &QT_Node::JoystickCmd_Callback, this);
    RC_Puber = n->advertise<common::RCCommand>("/quadruped/control/rc_cmd", 10);
}

void QT_Node::Run(void) {
    ros::MultiThreadedSpinner s(2);
    s.spin();
}

void QT_Node::FeetCmd_Callback(const sensor_msgs::JointStateConstPtr& msg) {
    rc_cmd.mode = RC_mode::PD_JOINT;
    // 获取QT测试数据
    rc_cmd.test_type[0] = QT_Foot; 
    rc_cmd.test_type[1] = std::stoi(msg->name[0]);

    for(int i = 0; i <  6; i ++){
        rc_cmd.test_data[i] = msg->position[i];
        rc_cmd.test_data[i+6] =  msg->velocity[i];
    }

    // 发布控制数据
    RC_Puber.publish(rc_cmd);
}

void QT_Node::JointsCmd_Callback(const sensor_msgs::JointStateConstPtr& msg) {
    rc_cmd.mode = RC_mode::PD_JOINT;
    // 获取QT测试数据
    for(int i = 0; i < msg->position.size(); i++){
        rc_cmd.test_data[i] = msg->position[i];
    }
    if(msg->position.size() == 1){
      rc_cmd.test_type[1] = std::stoi(msg->name[0]);
    }
    else{
      rc_cmd.test_type[1] = JOINTS_ALL;
    }
    rc_cmd.test_type[0] = QT_Joint;

    // 发布控制数据
    RC_Puber.publish(rc_cmd);
}

void QT_Node::JoystickCmd_Callback(const sensor_msgs::JointStateConstPtr& msg) {
    auto& cmd = msg->position;
    rc_cmd.mode = RC_mode::LOCOMOTION;
    for(int i = 0; i < 2; i ++) rc_cmd.p_des[i] = cmd[1+i];
    for(int i = 0; i < 2; i ++) rc_cmd.v_des[i] = cmd[3+i];
    rc_cmd.omega_des[2] = cmd[5];
    rc_cmd.height_variation = cmd[6];
    for(int i = 0; i < 2; i ++) rc_cmd.test_data[i] = cmd[7+i];
    rc_cmd.variable[0] = cmd[9];
    rc_cmd.variable[1] = cmd[10];
    rc_cmd.variable[2] = cmd[11];
    rc_cmd.test_type[0] = cmd[12];
    rc_cmd.test_type[1] = cmd[13];
    
    // 发布控制数据
    RC_Puber.publish(rc_cmd);
}

int main(int argc, char** argv) {
    // 节点初始化
    ros::init(argc, argv, "QT_Node", ros::init_options::AnonymousName);
    ros::NodeHandle n;

    // 创建控制器
    QT_Node qt_node(&n);

    qt_node.Run();

    return 0;
}

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
